/* 
 * File:   Track.cpp
 * Author: dave
 * 
 * Created on 2010年11月2日, 下午5:03
 */
#include "Track.h"

extern int numFrame;
extern int hdims;
extern int minT;


Track::Track(Camera *camera) {
    this->cam = camera;
    maxSize = 10;
    deadfrm = 3;
}

Track::Track(const Track& orig) {
}

Track::~Track() {
}

//初始化Track类
void Track::Init()
{
    float hranges_arr[]={0,180};
    float *hranges = hranges_arr;
    int hdims = 256;

    id = 0;objSize = 0;lostNum = 0;
    for(int i = 0; i < maxSize; ++i)
    {
        V[i].hist = cvCreateHist( 1, &hdims, CV_HIST_ARRAY, &hranges, 1 );
        V[i].image = cvCreateImage(cvGetSize(cam->pFrame), 8, 3);
        V[i].mask = cvCreateImage(cvGetSize(cam->pFrame), 8, 1);
        V[i].hsv = cvCreateImage( cvGetSize(cam->pFrame), 8, 3 );
        V[i].hue = cvCreateImage( cvGetSize(cam->pFrame), 8, 1 );
        V[i].backProject = cvCreateImage( cvGetSize(cam->pFrame), 8, 1 );
        V[i].deleted = true;
    }
}

//判断新物体comp是否被包含，废弃
bool Track::Match(CvConnectedComp comp)
{
    for(int i = 0; i < objSize; ++i)
    {
        if(V[i].deleted) continue;
        if(comp.rect.x > V[i].comp.rect.x + 10 || V[i].comp.rect.x > comp.rect.x + 10)
            continue;
        if(comp.rect.y > V[i].comp.rect.y + 10 || V[i].comp.rect.y > comp.rect.y + 10)
            continue;
        if(comp.area > V[i].comp.area * 2 || V[i].comp.area > comp.area * 2)
        if(comp.rect.height * comp.rect.width > V[i].comp.rect.height * V[i].comp.rect.width * 2
            || V[i].comp.rect.height * V[i].comp.rect.width > comp.rect.height * comp.rect.width * 2)
            continue;
        return true;
    }
    return false;
}

//判断矩形aCvRect a, CvRect b，b质心相互包含
//a为camshift后的矩形大小
bool Track::Match(CvRect a, CvRect b)
{
    int dis = a.x - b.x;
    int len;
    if(dis < 0) dis = -dis;
    len = (a.width + b.width) / 4;
    if(len < 20) len = 20;
    if(dis > len)
        return false;
    dis = a.y - b.y;
    if(dis < 0) dis = -dis;
    len = (a.height + b.height) / 4;
    if(len < 20) len = 20;
    if(dis > len)
        return false;

    //物体在边界附近变大不管
     dis = 20;
    if((a.x < dis || a.y < dis
            || a.x + a.width > this->cam->pFrame->width - dis
            || a.y + a.height > this->cam->pFrame->height - dis))
        return true;
    
    if(a.height * a.width > b.height * b.width * 2
                || b.height * b.width > a.height * a.width * 2)
        return false;

    return true;
}

//判断新物体obj是否被包含
bool Track::Match(trackObject obj)
{
    int x = (obj.pt0.x + obj.pt1.x) / 2, y = (obj.pt0.y + obj.pt1.y) / 2;
    for(int i = 0; i < objSize; ++i)
    {
        if(V[i].deleted) continue;
        int X = (V[i].pt0.x + V[i].pt1.x) / 2, Y = (V[i].pt0.y + V[i].pt1.y) / 2;
        if((x < V[i].pt0.x || x > V[i].pt1.x || y < V[i].pt0.y || y > V[i].pt1.y) &&
                (X < obj.pt0.x || X > obj.pt1.x || Y < obj.pt0.y || Y > obj.pt1.y))
            continue;
//        V[i].pt0 = obj.pt0;
//        V[i].pt1 = obj.pt1;
//        V[i].rect = obj.rect;
        V[i].matched = true;
        return true;
    }
    return false;
}

bool Track::IsNewObj(trackObject obj)
{
    for(int i = 0; i < objSize; ++i)
    {
        if(V[i].deleted) continue;
        if(!RectCross(V[i].pt0, V[i].pt1, obj.pt0, obj.pt1))
            continue;
        float rate = (obj.rect.width * obj.rect.height) / (V[i].rect.width * V[i].rect.height * 1.0);
        //面积太小认为噪点
        if(rate > 0.15)
        {
            // 计算质心偏移量
            int dx = (obj.pt0.x + obj.pt1.x) / 2;
            int dy = (obj.pt0.y + obj.pt1.y) / 2;
            //将质心移动
            V[i].rect.x = dx - V[i].rect.width/2;
            V[i].rect.y = dy - V[i].rect.height/2;
    //        为调试BUG而注释，应恢复
            if(V[i].rect.width < obj.rect.width)
            {
                V[i].rect.x -= (obj.rect.width - V[i].rect.width)/2;
                V[i].rect.width = obj.rect.width;
            }
            if(V[i].rect.height < obj.rect.height){
                V[i].rect.y -= (obj.rect.height - V[i].rect.height)/2;
                V[i].rect.height = obj.rect.height;
            }

    //废弃代码，希望通过前景图的面积和追踪中物体的面积进行修正
    //        if(rate > 0.8)
    //        {
    //            V[i].rect = obj.rect;
    //        }
    //        else if(rate > 0.5)
    //        {
    //            V[i].rect.x += dx;
    //            V[i].rect.y += dy;
    //        }
    //        else if(rate > 0.2)
    //        {
    //            V[i].rect.x += dx / 2;
    //            V[i].rect.y += dy / 2;
    //        }
    //
            //判断左上角是否出界
            if(V[i].rect.x < 0)
            {
                V[i].rect.width += V[i].rect.x;
                V[i].rect.x = 0;
            }
            if(V[i].rect.y < 0)
            {
                V[i].rect.height += V[i].rect.y;
                V[i].rect.y = 0;
            }

            //判断右下角是否出界
            int rx = V[i].rect.x + V[i].rect.width - 1;
            int ry = V[i].rect.y + V[i].rect.height - 1;
            if(rx >= this->cam->pFrame->width) {
                V[i].rect.width = this->cam->pFrame->width - V[i].rect.x - 1;
            }
            if(ry >= this->cam->pFrame->height){
                V[i].rect.height = this->cam->pFrame->height - V[i].rect.y - 1;
            }
            V[i].pt0 = cvPoint(V[i].rect.x, V[i].rect.y);
            V[i].pt1 = cvPoint(V[i].rect.x + V[i].rect.width - 1, V[i].rect.y + V[i].rect.height - 1);
        }
        
//        V[i].pt0 = obj.pt0;
//        V[i].pt1 = obj.pt1;
//        V[i].rect = obj.rect;
        V[i].matched = true;
        //记录物体前景中检测到的最大的物体区域
        if(V[i].locateSize < obj.rect.width * obj.rect.height){
            V[i].locateSize = obj.rect.width * obj.rect.height;
            V[i].locateX = (obj.pt0.x + obj.pt1.x) / 2;
            V[i].locateY = (obj.pt0.y + obj.pt1.y) / 2;
        }

        return true;
    }
    return false;
}

bool Track::lostMatch(CvRect a, CvRect b)
{
    int dis = a.x - b.x;
    int len;
    if(dis < 0) dis = -dis;
    len = (a.width + b.width)/2;
    if(len < 20) len = 20;
    if(dis > len)
        return false;
    dis = a.y - b.y;
    if(dis < 0) dis = -dis;
    len = (a.height + b.height)/2;
    if(len < 20) len = 20;
    if(dis > len)
        return false;

    if(a.height * a.width > b.height * b.width * 1.5)
            //|| b.height * b.width > a.height * a.width * 1.5)
        return false;
    return true;
}

bool Track::RectCross(CvPoint p11, CvPoint p12, CvPoint p21, CvPoint p22)
{
    return p11.x < p22.x && p21.x < p12.x && p11.y < p22.y && p21.y < p12.y;
}

//检查跟踪结果是否合法
bool Track::Check(CvConnectedComp* comp, CvBox2D* box)
{
    if(comp->rect.width <= 0 || comp->rect.height <= 0
            || box->size.width <= 0 || box->size.height <= 0
            || box->center.x < 0 || box->center.x >= cam->pFrame->width
            || box->center.y < 0 || box->center.y >= cam->pFrame->height)
        return false;
    return true;
}

//新物体跟踪，废弃
void Track::NewObject(vector<trackObject> obj)
{
//    IplImage *image = cvCreateImage(cvGetSize(cam->pFrame), 8, 3);
//    IplImage *mask = cvCreateImage(cvGetSize(cam->pFrame), 8, 1);
//
//    cvCopy(cam->pPre, image, 0);
//    cvCvtColor(image, cam->hsv, CV_BGR2HSV);
//    cvInRangeS(cam->hsv, cvScalar(0, 30, 10, 0),
//            cvScalar(180, 256, 256,0), mask );
//    cvSplit(cam->hsv, cam->hue, 0, 0, 0);
//
//    for(int i = 0; i < obj.size(); ++i)
//    {
//        //搜索边框
//        window = cvRect(obj[i].pt0.x, obj[i].pt0.y,
//                obj[i].pt1.x - obj[i].pt0.x + 1, obj[i].pt1.y - obj[i].pt0.y + 1);
//        //设置图像的感兴趣区域
//        cvSetImageROI(cam->hue, window);
//        cvSetImageROI(mask, window);
//        cvCalcHist(&cam->hue, cam->hist, 0, mask);
//        float max_val = 0.0;
//        cvGetMinMaxHistValue(cam->hist, 0, &max_val, 0, 0 );
//        cvConvertScale(cam->hist->bins, cam->hist->bins,
//                max_val ? 255. / max_val : 0, 0 );
//        cvResetImageROI(cam->hue);
//        cvResetImageROI(mask);
//
//        //计算目标直方图的反向投影
//        try {
//            cvCalcBackProject(&(cam->hue), cam->backProject, cam->hist);
//        } catch(...) {
//
//        }
//
//        //蒙板
//        cvAnd(cam->backProject, mask, cam->backProject, 0);
//        //执行CamShift算法跟踪目标
//        cvCamShift(cam->backProject, window,
//                cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ),
//                &comp, &box);
//
//        if(!Match(comp))
//        {
//            trackObject tmpObj = obj[i];
//            tmpObj.id = id++;
//            tmpObj.lostfrm = 0;
//            tmpObj.comp.rect = window;
//            tmpObj.comp.area = window.height * window.width;
//            //保存跟踪结果
//            V.push_back(tmpObj);
//        }
//    }
//    cvReleaseImage(&image);
//    cvReleaseImage(&mask);
}

//添加新物体到跟踪列表
void Track::Add(trackObject obj)
{
//    if(objSize == maxSize) return;
//    V[objSize].id = id++;
//    V[objSize].lostfrm = 0;
//    V[objSize].matched = true;
//    V[objSize].rect = obj.rect;
//    V[objSize].pt0 = obj.pt0;
//    V[objSize].pt1 = obj.pt1;
//    objSize++;
    for(int i = 0; i < lostNum; ++i)
    {
        if(lostObj[i].deleted) continue;
        if(lostMatch(obj.rect, lostObj[i].rect))
        {
            lostObj[i].deleted = true;
            for(int j = 0; j < objSize; ++j)
            {
                if(V[j].id == lostObj[i].id)
                {
                    V[j].deleted = false;
                    //以当前locate出的为中心，不改变大小进行平移
                    V[j].rect = lostObj[i].rect;
                    V[j].pt0 = lostObj[i].pt0;
                    V[j].pt1 = lostObj[i].pt1;

                    int x = (obj.pt0.x + obj.pt1.x) / 2;
                    int y = (obj.pt0.y + obj.pt1.y) / 2;
                    //将质心移动
                    V[j].rect.x = x - V[j].rect.width/2;
                    V[j].rect.y = y - V[j].rect.height/2;
//                    V[j].rect.width = (V[j].rect.width+obj.rect.width)/2;
//                    V[j].rect.height = (V[j].rect.height+obj.rect.height)/2;

                    //判断左上角是否出界
                    if(V[j].rect.x < 0)
                    {
                        V[j].rect.width += V[j].rect.x;
                        V[j].rect.x = 0;
                    }
                    if(V[j].rect.y < 0)
                    {
                        V[j].rect.height += V[j].rect.y;
                        V[j].rect.y = 0;
                    }

                    //判断右下角是否出界
                    int rx = V[j].rect.x + V[j].rect.width - 1;
                    int ry = V[j].rect.y + V[j].rect.height - 1;
                    if(rx >= this->cam->pFrame->width) {
                        V[j].rect.width = this->cam->pFrame->width - V[j].rect.x - 1;
                    }
                    if(ry >= this->cam->pFrame->height){
                        V[j].rect.height = this->cam->pFrame->height - V[j].rect.y - 1;
                    }
                    V[j].pt0 = cvPoint(V[j].rect.x, V[j].rect.y);
                    V[j].pt1 = cvPoint(V[j].rect.x + V[j].rect.width - 1, V[j].rect.y + V[j].rect.height - 1);


                    break;
                }
            }
            return;
        }
    }
    if(objSize < maxSize)
    {
        V[objSize].id = id++;
        V[objSize].lostfrm = 0;
        V[objSize].showID = objSize;
        V[objSize].matched = true;
        V[objSize].deleted = false;
        V[objSize].rect = obj.rect;
        V[objSize].pt0 = obj.pt0;
        V[objSize].pt1 = obj.pt1;
        objSize++;
    } else {
        for(int i = 0; i < objSize; ++i)
        {
            if(V[i].deleted)
            {
                V[i].id = id++;
                V[i].lostfrm = 0;
                V[i].showID = i;
                V[i].matched = true;
                V[i].deleted = false;
                V[i].rect = obj.rect;
                V[i].pt0 = obj.pt0;
                V[i].pt1 = obj.pt1;
                return;
            }
        }
    }
}
void Track::AddLost(trackObject obj)
{
    if(lostNum < maxSize * 3)
    {
        lostObj[lostNum].id = obj.id;
        lostObj[lostNum].lostfrm = 0;
        lostObj[lostNum].matched = true;
        lostObj[lostNum].deleted = false;
        lostObj[lostNum].rect = obj.rect;
        lostObj[lostNum].pt0 = obj.pt0;
        lostObj[lostNum].pt1 = obj.pt1;
        lostNum++;
    } else {
        for(int i = 0; i < lostNum; ++i)
        {
            if(lostObj[i].deleted)
            {
                lostObj[i].id = obj.id;
                lostObj[i].lostfrm = 0;
                lostObj[i].matched = true;
                lostObj[i].deleted = false;
                lostObj[i].rect = obj.rect;
                lostObj[i].pt0 = obj.pt0;
                lostObj[i].pt1 = obj.pt1;
                return;
            }
        }
    }
}

//丢弃跟踪丢失的物体
void Track::Erase()
{
    for(int i = 0; i < objSize; ++i)
    {
        if(!V[i].deleted && V[i].lostfrm == deadfrm)
        {
            V[i].deleted = true;
        }
    }
    for(int i = 0; i < lostNum; ++i)
    {
        if(!lostObj[i].deleted && lostObj[i].lostfrm++ == deadfrm)
        {
            lostObj[i].deleted = true;
        }
    }
//    int i = 0, j = 0;
//    for( ; i < objSize; ++i)
//    {
//        if(V[i].lostfrm < deadfrm)
//        {
//            if(i != j)
//            {
//                V[j].id = V[i].id;
//                V[j].lostfrm = V[i].lostfrm;
//                V[j].matched = V[i].matched;
//                V[j].pt0 = V[i].pt0;
//                V[j].pt1 = V[i].pt1;
//                V[j].rect = V[i].rect;
//            }
//            ++j;
//        }
//    }
//    objSize = j;
}

//对指定物体进行CamShift跟踪
void Track::CamShift(trackObject * obj)
{
    CvRect tmp = obj->rect;
//    obj->rect.x += obj->rect.width / 8;
//    obj->rect.y += obj->rect.height / 8;
//    obj->rect.width = obj->rect.width*3/4;
//    obj->rect.height = obj->rect.height*3/4;

    cvCopy(cam->pFrame, obj->image, 0);
    cvCvtColor(obj->image, obj->hsv, CV_BGR2HSV);
    cvInRangeS(obj->hsv, cvScalar(0, 30, 10, 0),
            cvScalar(180, 256, 256,0), obj->mask );
    cvSplit(obj->hsv, obj->hue, 0, 0, 0);
    
    //设置图像的感兴趣区域
    cvSetImageROI(obj->hue, obj->rect);
    cvSetImageROI(obj->mask, obj->rect);

    cvCalcHist(&obj->hue, obj->hist, 0, obj->mask);
    float max_val = 0.0;
    cvGetMinMaxHistValue(obj->hist, 0, &max_val, 0, 0 );
    cvConvertScale(obj->hist->bins, obj->hist->bins,
            max_val ? 255. / max_val : 0, 0 );
    cvResetImageROI(obj->hue);
    cvResetImageROI(obj->mask);
    //计算目标直方图的反向投影
    cvCalcBackProject(&(obj->hue), obj->backProject, obj->hist);
    
    //蒙板
    cvAnd(obj->backProject, obj->mask, obj->backProject, 0);
    //执行CamShift算法跟踪目标
    cvCamShift(obj->backProject, obj->rect,
            cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 5, 1 ),
            &obj->comp, &obj->box);

    obj->rect = tmp;
}

//处理新检测物体
void Track::TrackNew(vector<trackObject> &obj)
{
    for(int i = 0; i < obj.size(); ++i)
    {
        if(!IsNewObj(obj[i]))
        {
//            if(obj[i].pt1.x < obj[i].pt0.x || obj[i].pt1.y < obj[i].pt0.y)
//                printf("Error:%d %d %d %d %d\t", obj[i].id, obj[i].pt0.x, obj[i].pt0.y, obj[i].pt1.x, obj[i].pt1.y);

            obj[i].matched = false;
            //保存跟踪结果
            //Add(obj[i]);
            //printf("%d %d %d %d %d\t", obj[i].id, obj[i].pt0.x, obj[i].pt0.y, obj[i].pt1.x, obj[i].pt1.y);
            //printf("%d %d %d %d\n", obj[i].rect.x, obj[i].rect.y, obj[i].rect.width, obj[i].rect.height);
        } else {
            obj[i].matched = true;
        }
    }
}

//跟踪目标物体
void Track::TrackExist(vector<trackObject> &obj)
{
    int i, k = objSize;
    for(i = 0; i < objSize; ++i)
    {
        if(V[i].deleted) continue;
        CamShift(&V[i]);
    }

    for(i = 0; i < objSize; ++i)
    {
        if(V[i].deleted) continue;
        if(Check(&V[i].comp, &V[i].box) && Match(V[i].comp.rect, V[i].rect))
        {
            Resize(V[i]);
            if(V[i].rect.x == 0 && V[i].locateSize == -1){
                AddLost(V[i]);
                V[i].deleted = true;
            }
        } else {
            AddLost(V[i]);
            V[i].deleted = true;
            //V[i].lostfrm++;
        }
        V[i].matched = false;
        //printf("%d %d %d %d\n", V[i].pt0.x, V[i].pt0.y, V[i].pt1.x, V[i].pt1.y);
    }
        //初始话追踪物体和前景物体匹配前，无最大前景物体的中心位置，此参数为参数计算模块计算视差提供参考
    for(int i = 0; i < objSize; i++){
        if(V[i].deleted) continue;
        V[i].locateSize = -1;
    }

    ////////对上一帧的物体做完追踪后再进行新物体判定//////////////////
    //为什么没有被locate匹配的图像修正回去
    TrackNew(obj);
    
    //printf("objSize before Add: %d\n", objSize);
    for(int i = 0; i < obj.size(); ++i)
    {
        if(!obj[i].matched && Match(obj[i]))
        {
            obj[i].matched = true;
        }
    }
    for(i = 0; i < obj.size(); ++i)
    {
        if(!obj[i].matched)
        {
            obj[i].lostfrm = 0;
            Add(obj[i]);
        }
    }
    //printf("objSize after Add: %d\n", objSize);

    for(i = k; i < objSize; ++i)
    {
        if(V[i].deleted) continue;
        CamShift(&V[i]);
    }

    for(i = k; i < objSize; ++i)
    {
        if(V[i].deleted) continue;
        if(Check(&V[i].comp, &V[i].box))// && Match(V[i].comp.rect, V[i].rect))
        {
            Resize(V[i]);
        } else {
            AddLost(V[i]);
            V[i].deleted = true;
            //V[i].lostfrm++;
        }
        V[i].matched = false;
    }
}

void Track::Resize(trackObject & obj)
{
    CvRect rect = obj.comp.rect;
    int pre = obj.rect.width * obj.rect.height;
    int now = rect.width * rect.height;
    int lostNum = -3;
    if( now < 0.8 * pre)
//    if(obj.rect.width * obj.rect.height > rect.width * rect.height)
    {
        //当前搜索到的矩形变小幅度大太时回复之前的大小
        rect.width = obj.rect.width * 0.8;
        rect.height = obj.rect.height * 0.8;
    }
    //控制追踪框放大不能超过400
    else if(now - pre > 400 && obj.lostfrm > lostNum){

        int dis = 20;
        //边界部分不管
//        if(!(rect.x < dis || rect.y < dis
//                || rect.x+rect.width > this->cam->pFrame->width - dis
//                || rect.y + rect.height > this->cam->pFrame->height - dis))
//        {
            double fac = 400.0 / (now-pre);
            int width, heigh;
            width = fac * (rect.width - obj.rect.width);
            heigh = fac * (rect.height - obj.rect.height);

            //当处于边界时，应该将物体平移到边界位置,这里只处理了左边界，这里偷懒了
            
            int rx, ry;
            int centerx, centery;
            centerx = rect.x + rect.width/2;
            centery = rect.y + rect.height/2;

            //当前搜索到的矩形变小幅度大太时回复之前的大小
            rect.width = obj.rect.width + width;
            rect.height = obj.rect.height + heigh;
            if(rect.x != 0){
                rect.x = centerx - rect.width/2;
            }
            rect.y = centery - rect.height/2;
            //判断左上角是否出界
            if(rect.x < 0)
            {
                rect.width += rect.x;
                rect.x = 0;
            }
            if(rect.y < 0)
            {
                rect.height += rect.y;
                rect.y = 0;
            }

            //判断是否出了视窗窗口
            rx = rect.x + rect.width - 1;
            ry = rect.y + rect.height - 1;
            if(rx >= this->cam->pFrame->width) {
                rect.width = this->cam->pFrame->width - rect.x - 1;
            }
            if(ry >= this->cam->pFrame->height){
                rect.height = this->cam->pFrame->height - rect.y - 1;
            }
//        }
    }
    else if( now > 1.2 * pre  && obj.lostfrm > lostNum)
    {
        int dis = 20;

        //边界部分不管
        if(!(rect.x < dis || rect.y < dis
                || rect.x+rect.width > this->cam->pFrame->width - dis
                || rect.y + rect.height > this->cam->pFrame->height - dis))
        {
            int rx, ry;
            int centerx, centery;
            centerx = rect.x + rect.width/2;
            centery = rect.y + rect.height/2;

            //当前搜索到的矩形变小幅度大太时回复之前的大小
            rect.width = obj.rect.width * 1.1;
            rect.height = obj.rect.height * 1.1;
            rect.x = centerx - rect.width/2;
            rect.y = centery - rect.height/2;
            //判断左上角是否出界
            if(rect.x < 0)
            {
                rect.width += rect.x;
                rect.x = 0;
            }
            if(rect.y < 0)
            {
                rect.height += rect.y;
                rect.y = 0;
            }

            //判断是否出了视窗窗口
            rx = rect.x + rect.width - 1;
            ry = rect.y + rect.height - 1;
            if(rx >= this->cam->pFrame->width) {
                rect.width = this->cam->pFrame->width - rect.x - 1;
            }
            if(ry >= this->cam->pFrame->height){
                rect.height = this->cam->pFrame->height - rect.y - 1;
            }
        }
    }
    obj.pt0 = cvPoint(rect.x, rect.y);
    obj.pt1 = cvPoint(rect.x + rect.width - 1, rect.y + rect.height - 1);
    obj.rect = rect;

    ////////加入物体稳定判断并保留信息/////////////////////////////
    if(obj.lostfrm == 0){
        //第一次给物体赋值
        obj.weigh = rect.width;
        obj.heigh = rect.height;
    }
    else if(obj.lostfrm < 0 && obj.lostfrm > lostNum){
        //比较变化律
        double facW = obj.weigh - rect.width;
        double facH = obj.heigh - rect.height;

        if(facW < 0) facW = -facW;
        if(facH < 0) facH = -facH;
        //不处于稳定时重新赋值
        if(facW / obj.weigh > 0.05 || facH / obj.heigh > 0.05){
            obj.weigh = rect.width;
            obj.heigh = rect.height;
            obj.lostfrm = 0;
        }
        else{
            obj.weigh = (obj.weigh + rect.width) / 2;
            obj.heigh = (obj.heigh + rect.height) / 2;
        }
    }
    else{
        obj.rect.width = obj.weigh;
        obj.rect.height = obj.heigh;
        
        //判断是否出了视窗窗口
        int rx, ry;
        rx = obj.rect.x + obj.rect.width - 1;
        ry = obj.rect.y + obj.rect.height - 1;
        if(rx >= this->cam->pFrame->width) {
            obj.rect.width = this->cam->pFrame->width - obj.rect.x - 1;
        }
        if(ry >= this->cam->pFrame->height){
            obj.rect.height = this->cam->pFrame->height - obj.rect.y - 1;
        }
    }
    obj.lostfrm--;

    //限制长宽比不能超过2
    if(obj.rect.width*1.0 / obj.rect.height > 2) {
        int width = obj.rect.height*2;
        obj.rect.x -= (obj.rect.width - width)/2;
        obj.rect.width = width;
    }

    else if(obj.rect.height * 1.0 / obj.rect.width > 2){
        int height = obj.rect.width * 2;
        obj.rect.y -= (obj.rect.height - height) / 2;
        obj.rect.height = height;
    }
    cout<<"hello "<<obj.lostfrm<<" weight: "<<obj.weigh<<" height: "<<obj.heigh<<endl;
}

void Track::Merge()
{
    for(int i = 0; i < objSize; ++i)
    {
        if(V[i].deleted) continue;
        for(int j = i + 1; j < objSize; ++j)
        {
            if(V[j].deleted) continue;
            if(RectCross(V[i].pt0, V[i].pt1, V[j].pt0, V[j].pt1))
            {
                if(V[i].rect.width * V[i].rect.height
                        < V[j].rect.width * V[j].rect.height)
                {
                    V[i].deleted = true;
                    break;
                }
                V[j].deleted = true;
            }
        }
    }
}

//Track类程序入口
void Track::Run(vector<trackObject> &obj)
{
    //NewObject(obj);
    //第一遍只是通过locate的物体对追踪中物体修正
     //初始话追踪物体和前景物体匹配前，无最大前景物体的中心位置，此参数为参数计算模块计算视差提供参考
    for(int i = 0; i < objSize; i++){
        if(V[i].deleted) continue;
        V[i].locateSize = -1;
    }
    TrackNew(obj);
    for(int i = 0; i < obj.size(); i++)
        obj[i].matched = false;

    //追踪函数中真正调用匹配
    TrackExist(obj);

    Merge();

    Erase();

    //printf("TotalObj:%d\t\n", objSize);
}

//在画面中标示跟踪物体
void Track::Draw(IplImage * image, CvScalar color)
{
    char s[8];

    CvFont * font = &cvFont(2);
    for(int i = 0; i < objSize; ++i)
    {
        if(V[i].deleted) continue;
        printf("id:%d\tlostfrm:%d\t%d %d\n", V[i].id, V[i].lostfrm, V[i].pt1.x, V[i].pt1.y);
        cvRectangle(image, V[i].pt0, V[i].pt1, color, 2);
        sprintf(s, "%d", i);
        cvPutText(image, s, V[i].pt1, font, color);
    }
}